/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "rtthread.h"
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "maths.h"
#include "drv_pin.h"
#include "fc_state.h"
// #include "si24r1_port.h"
#include "si24r1.h"
#include "rc.h"
#include "zino.h"

#define DBG_TAG "rc"
#define DBG_LVL DBG_LOG

#include <rtdbg.h>

ZINO_CONFIG_REGISTER(struct rc_pair_data_s, pair_data, 3, 1);
uint8_t rxbuffer[RTX_BUFFER_SIZE]; // 接收缓冲区
uint8_t pair_channel = 33;
uint8_t pair_addr[5] = "UAVI1";
uint8_t pair_handshake_byte = 0xdc; // TOPS 专有，升级跳频功能
uint8_t pair_timeout_second = 20;
uint8_t test_address[5] = "te0st";
uint8_t test_channel = 80;
rc_t rchandle;
#if 0
void si24r1_init()
{
    hw_si24r1_clear_ce(); // 清除CE
    // hw_si24r1_write_reg(SI24R1_REG_CONFIG, RX_IRQ_ENABLE | TX_IRQ_ENABLE | MAX_RT_IRQ_ENABLE | CRC_ON | CRC_1bytes | PWR_DOWN | TX_ON); //CRC,8 bit,Power on,RX
    hw_si24r1_write_reg(SI24R1_REG_ENAA, 0X01);                                             // 使能通道0应答
    hw_si24r1_write_reg(SI24R1_REG_EN_RXADDR, 0X01);                                        // 使能通道0
    hw_si24r1_write_reg(SI24R1_REG_SETUP_AW, 3);                                            // 通信地址宽度5
    hw_si24r1_write_reg(SI24R1_REG_SETUP_RETR, (1 << 4) | (5));                             // 重发延时250us+1*250us,自动重发10次
    hw_si24r1_write_reg(SI24R1_REG_RF_CH, rchandle.rfChannel);                              // 射频信道2400MHz+40MHz
    hw_si24r1_write_reg(SI24R1_REG_RF_SETUP, SI24R1_RF_SETUP_2Mbps | SI24R1_RF_SETUP_7dBm); // 1Mbps,7dBm
    hw_si24r1_write_reg(SI24R1_REG_RX_PW_P0, RTX_BUFFER_SIZE);                              // 通道0接收数据字节数
    hw_si24r1_write_reg(SI24R1_REG_FEATURE, SI24R1_FEATURE_EN_ACK_PAYd | SI24R1_FEATURE_EN_DPL);
    hw_si24r1_tx_address(rchandle.address);
    hw_si24r1_rx_address(rchandle.address);
}
#endif

void check_all_regs()
{
    uint8_t rx[5];
    LOG_D("\nAll reg:");
    LOG_D("SI24R1_REG_CONFIG:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_CONFIG));
    LOG_D("SI24R1_REG_ENAA:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_ENAA));
    LOG_D("SI24R1_REG_EN_RXADDR:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_EN_RXADDR));
    LOG_D("SI24R1_REG_SETUP_AW:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_SETUP_AW));
    LOG_D("SI24R1_REG_SETUP_RETR:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_SETUP_RETR));
    LOG_D("SI24R1_REG_RF_CH:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_RF_CH));
    LOG_D("SI24R1_REG_RF_SETUP:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_RF_SETUP));
    LOG_D("SI24R1_REG_RX_PW_P0:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_RX_PW_P0));
    rt_thread_mdelay(1);
    LOG_D("SI24R1_REG_DYNPD:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_DYNPD));
    LOG_D("SI24R1_REG_FEATURE:0x%02X", (int)hw_si24r1_read_reg(SI24R1_REG_FEATURE));
    hw_si24r1_read_reg_buf(SI24R1_REG_RX_ADDR_P0, rx, 5);
    LOG_D("SI24R1_REG_RX_ADDR_P0:0x%02X 0x%02X 0x%02X 0x%02X 0x%02X", (int)rx[0], (int)rx[1], (int)rx[2], (int)rx[3], (int)rx[4]);
    hw_si24r1_read_reg_buf(SI24R1_REG_TX_ADDR, rx, 5);
    LOG_D("SI24R1_REG_TX_ADDR   :0x%02X 0x%02X 0x%02X 0x%02X 0x%02X", (int)rx[0], (int)rx[1], (int)rx[2], (int)rx[3], (int)rx[4]);
}
#if 0
void rc_send_data(struct rchandle *rc)
{
    hw_si24r1_clear_ce();
    // memcpy(rchandle.address, "test4", 5);
    // rchandle.channel = 130;
    //  LOG_D("rc->channel:%02X\n",(int)rchandle.rfChannel);
    hw_si24r1_write_reg(SI24R1_REG_CONFIG, EN_CRC | CRCO_2BYTE | PWR_UP);
    hw_si24r1_fifo_flush();
    hw_si24r1_write_reg(SI24R1_REG_SETUP_AW, 0X03);
    hw_si24r1_write_reg(SI24R1_REG_ENAA, 0X01);
    hw_si24r1_write_reg(SI24R1_REG_EN_RXADDR, 0X01);
    hw_si24rq_reg_write_buf(SI24R1_REG_TX_ADDR | SI24R1_CMD_W_REGISTER, rc->address, 5);
    hw_si24rq_reg_write_buf(SI24R1_REG_RX_ADDR_P0 | SI24R1_CMD_W_REGISTER, rc->address, 5);
    hw_si24rq_reg_write_buf(SI24R1_CMD_W_TX_PAYLOAD, rc->tx_data.raw, RTX_BUFFER_SIZE);
    hw_si24r1_write_reg(SI24R1_REG_FEATURE, SI24R1_FEATURE_EN_DPL | SI24R1_FEATURE_EN_ACK_PAYd); // 使能动态负载
    hw_si24r1_write_reg(SI24R1_REG_DYNPD, 0x01);                                                 // 开启通道0动态负载
    hw_si24r1_write_reg(SI24R1_REG_SETUP_RETR, 0x0A);                                            // 250us,10次
    hw_si24r1_write_reg(SI24R1_REG_RF_CH, rc->rfChannel);
    hw_si24r1_write_reg(SI24R1_REG_RF_SETUP, SI24R1_RF_SETUP_2Mbps | SI24R1_RF_SETUP_7dBm);
    hw_si24r1_write_reg(SI24R1_REG_STATUS, CLEAR_RX_IRQ | CLEAR_TX_IRQ | CLEAR_MAX_RT_IRQ);
    hw_si24r1_set_ce();
}
#endif

void rc_key_action_switch(void)
{
    LOG_D("rchandle.mode:%d\n", rchandle.mode);
    if (rchandle.mode != RC_MODE_PAIRING)
    {
        rc_into_pair_mode();
    }
    else
    {
        rc_into_trans_mode();
    }
}
void rc_rx_mode()
{
    hw_si24r1_clear_ce();
    hw_si24r1_fifo_clear();
    hw_si24r1_write_reg(SI24R1_REG_CONFIG, EN_CRC | PWR_UP | PRIM_RX);
    hw_si24r1_write_reg(SI24R1_REG_ENAA, 0X01);
    hw_si24r1_write_reg(SI24R1_REG_EN_RXADDR, 0X01);
    hw_si24r1_write_reg(SI24R1_REG_SETUP_AW, 0x03);
    hw_si24r1_write_reg(SI24R1_REG_SETUP_RETR, 0x0A);          // 250us,10次
    hw_si24r1_write_reg(SI24R1_REG_RF_CH, rchandle.rfChannel); // 250us,10次
    hw_si24r1_write_reg(SI24R1_REG_RF_SETUP, SI24R1_RF_SETUP_1Mbps | SI24R1_RF_SETUP_7dBm);
    hw_si24r1_write_reg(SI24R1_REG_STATUS, CLEAR_RX_IRQ | CLEAR_TX_IRQ | CLEAR_MAX_RT_IRQ);

    hw_si24r1_tx_address(rchandle.address);
    hw_si24r1_rx_address(rchandle.address);

    hw_si24r1_write_reg(SI24R1_REG_RX_PW_P0, RTX_BUFFER_SIZE);
    hw_si24r1_write_reg(SI24R1_REG_DYNPD, 0x01);                                                 // 开启通道0动态负载
    hw_si24r1_write_reg(SI24R1_REG_FEATURE, SI24R1_FEATURE_EN_DPL | SI24R1_FEATURE_EN_ACK_PAYd); // 使能动态负载

    // hw_si24r1_write_ack_payload(rchandle.tx_data.raw, 8); // fill ack payload
    hw_si24r1_set_ce();
}

#if 0
void rc_get_data(struct rchandle *rc)
{
    hw_si24r1_clear_ce();
    memcpy(rchandle.address, "tes01", 5);
    rchandle.rfChannel = 120;
    // rchandle.channel = 125;
    hw_si24r1_write_reg(SI24R1_REG_SETUP_AW, 0X03);
    hw_si24rq_reg_write_buf(SI24R1_REG_TX_ADDR | SI24R1_CMD_W_REGISTER, rc->address, 5);
    hw_si24rq_reg_write_buf(SI24R1_REG_RX_ADDR_P0 | SI24R1_CMD_W_REGISTER, rc->address, 5);
    hw_si24r1_write_reg(SI24R1_REG_EN_RXADDR, 0x01); // 使能通道0
    hw_si24r1_write_reg(SI24R1_REG_RF_CH, rc->rfChannel);
    hw_si24r1_write_reg(SI24R1_REG_RX_PW_P0, RTX_BUFFER_SIZE);
    hw_si24r1_write_reg(SI24R1_REG_SETUP_AW, 0x03);
    hw_si24r1_write_reg(SI24R1_REG_FEATURE, SI24R1_FEATURE_EN_DPL); // 使能动态负载
    hw_si24r1_write_reg(SI24R1_REG_DYNPD, 0x01);                    // 开启通道0动态负载
    hw_si24r1_write_reg(SI24R1_REG_RF_SETUP, SI24R1_RF_SETUP_2Mbps | SI24R1_RF_SETUP_4dBm);
    hw_si24r1_write_reg(SI24R1_REG_CONFIG, EN_CRC | CRCO_2BYTE | PWR_UP | PRIM_RX);
    hw_si24r1_set_ce();
}
#endif

void rc_pair_mode_rx_handle(void)
{
    static uint8_t ackCnt = 0;
    ackCnt++;
    if (rchandle.rx_data.v.head == WL_MSG_HEAD_PAIR_REQ) // 回复新地址
    {
        rchandle.newAddress[0] = rchandle.rx_data.raw[2];
        rchandle.newAddress[1] = rchandle.rx_data.raw[3];
        rchandle.newAddress[2] = rchandle.rx_data.raw[4];
        rchandle.newAddress[3] = rchandle.rx_data.raw[5];
        rchandle.newAddress[4] = rchandle.rx_data.raw[6];
        rchandle.newRfChannel = rchandle.rx_data.raw[7];

        rt_memset(rchandle.tx_data.raw, 0, sizeof(rchandle.tx_data.raw));
        rchandle.tx_data.v.head = WL_MSG_HEAD_PAIR_REQ;
        rchandle.tx_data.v.index = ackCnt;
        rchandle.tx_data.raw[2] = rchandle.newAddress[0];
        rchandle.tx_data.raw[3] = rchandle.newAddress[1];
        rchandle.tx_data.raw[4] = rchandle.newAddress[2];
        rchandle.tx_data.raw[5] = rchandle.newAddress[3];
        rchandle.tx_data.raw[6] = rchandle.newAddress[4];
        rchandle.tx_data.raw[7] = rchandle.newRfChannel;

        rchandle.pairConfirmTime = 0;
        ackCnt = 0;

        LOG_D("new address:%02X-%02x-%02X-%02X-%02X, channel:%d\n",
              rchandle.newAddress[0],
              rchandle.newAddress[1],
              rchandle.newAddress[2],
              rchandle.newAddress[3],
              rchandle.newAddress[4],
              rchandle.newRfChannel);
        // LOG_D("WL_MSG_HEAD_PAIR_REQ:%d\n", rchandle.tx_data.v.index);
    }
    else if (rchandle.rx_data.v.head == WL_MSG_HEAD_PAIR_COMFIR) // 对方确认
    {

        rchandle.tx_data.v.head = WL_MSG_HEAD_PAIR_COMFIR;
        rchandle.tx_data.v.index = ackCnt;
        LOG_D("WL_MSG_HEAD_PAIR_COMFIR:%d\n", rchandle.rx_data.v.index);
        if (rchandle.rx_data.v.index > 2)
        {
            rchandle.pairConfirmTime = 100;
        }
    }

    rt_memset(rchandle.rx_data.v.channel, 0, sizeof(rchandle.rx_data.v.channel));
}
void rc_into_pair_mode()
{
    if (rchandle.mode != RC_MODE_PAIRING)
    {
        rchandle.pairTime = 0;
        rchandle.mode = RC_MODE_PAIRING;
        rchandle.rfChannel = pair_channel;
        rchandle.pairConfirmTime = 0;
        rchandle.pairTime = 0;
        memcpy(rchandle.address, pair_addr, 5);
        LOG_D("rc_into_pair_mode \n ");
        rc_rx_mode();
        check_all_regs();
        FC_STATE_SET(FC_RC_PAIRING);
    }
}
void rc_into_trans_mode()
{
    if (rchandle.mode != RC_MODE_NORMAL)
    {
        rchandle.mode = RC_MODE_NORMAL;
        memcpy(rchandle.address, pair_data()->address, 5);
        rchandle.rfChannel = pair_data()->rf_channel;
        LOG_D("rc_into_trans_mode \n ");
        rc_rx_mode();

        check_all_regs();

        FC_STATE_CLEAR(FC_RC_LOST);
        FC_STATE_CLEAR(FC_RC_PAIRING);
    }
}

#define MSB_SWAP(x) (((x) >> 8) | ((x) << 8))
#define RC_TASK_INTERVAL_MS 1

void rc_task_1s(void)
{
    rchandle.rssi = rchandle.rx_cnt;
    rchandle.rx_cnt = 0;

    static uint8_t TxRssi;

    TxRssi = rchandle.tx_cnt;
    rchandle.tx_cnt = 0;
    if (rchandle.mode == RC_MODE_PAIRING)
    {
        rchandle.pairTime++;
        if (rchandle.pairTime > 250)
        {
            LOG_D("pair fail, timeout!\n");
            rc_into_trans_mode();
        }
    }
    // LOG_D("T/R: [%3d/%3d]", TxRssi, rchandle.rssi);

    if (rchandle.rssi <= 1)
    {
        rt_memset(rchandle.rx_data.raw, 0, sizeof(rchandle.rx_data.raw));
        FC_STATE_SET(FC_RC_LOST);
    }
    else
    {
        FC_STATE_CLEAR(FC_RC_LOST);
    }
}
void rc_task_1ms(void)
{
    // uint8_t status = hw_si24r1_read_reg(SI24R1_REG_STATUS);
    // static uint8_t status_pre = 0;
    // if (status_pre != status)
    // {
    //     status_pre = status;
    //     LOG_D("status:%02X\n", status);
    // }
    if (pin_read(SI24R1_IRQ_PIN) == 0)
    {
        uint8_t status = hw_si24r1_read_reg(SI24R1_REG_STATUS);
        // hw_si24r1_write_ack_payload(rchandle.tx_data.raw, 8);
        if (status & CLEAR_RX_IRQ)
        {
            hw_si24r1_clr_rxit();
            hw_si24r1_fifo_read(rxbuffer, RTX_BUFFER_SIZE);
            uint8_t crc = crc8_dvb_s2_update(CRC_MAGICBEE, rxbuffer, RTX_BUFFER_SIZE - 1);
            hw_si24r1_fifo_clear();
            if (crc == rxbuffer[RTX_BUFFER_SIZE - 1])
            {
                rchandle.rx_cnt++;
                memcpy(rchandle.rx_data.raw, rxbuffer, RTX_BUFFER_SIZE);
                if (rchandle.mode == RC_MODE_PAIRING)
                {
                    rc_pair_mode_rx_handle();
                }
                else
                {
                    rchandle.tx_data.v.head = WL_MSG_HEAD_FC_STATUS;
                    rchandle.tx_data.v.index = 0;
                    rchandle.tx_data.raw[2] = fc_states >> 24;
                    rchandle.tx_data.raw[3] = fc_states >> 16;
                    rchandle.tx_data.raw[4] = fc_states >> 8;
                    rchandle.tx_data.raw[5] = fc_states;
                    rchandle.tx_data.raw[6] = rchandle.tx_data.raw[2] ^ rchandle.tx_data.raw[3] ^ rchandle.tx_data.raw[4] ^ rchandle.tx_data.raw[5];
                }
            }
            hw_si24r1_write_ack_payload(rchandle.tx_data.raw, 8); // fill ack payload
        }
        else if (status & CLEAR_TX_IRQ)
        {
            // LOG_D("CLEAR_TX_IRQ\n");
            hw_si24r1_clr_txit();
            rchandle.tx_cnt++;
        }
        else if (status & CLEAR_MAX_RT_IRQ)
        {
            // LOG_D("CLEAR_MAX_RT_IRQ\n");
            hw_si24r1_clr_maxit();
        }
    }
    else
    {
        rchandle.lost_cnt++;
        if (rchandle.lost_cnt >= 1000)
        {
            rchandle.lost_cnt = 0;
            rc_rx_mode();
            // LOG_D("rc lost!\n");
        }
    }
    if (rchandle.pairConfirmTime)
    {
        rchandle.pairConfirmTime--;
        if (rchandle.pairConfirmTime == 0)
        {
            pair_data()->flag = RC_PAIRED_FLAG;

            memcpy(pair_data()->address, rchandle.newAddress, 5);
            pair_data()->rf_channel = rchandle.newRfChannel;

            memcpy(rchandle.address, rchandle.newAddress, 5);
            rchandle.rfChannel = rchandle.newRfChannel;

            zino_config_save_all();

            LOG_D("pair success!\n");
            LOG_D("Load address:%02X-%02X-%02X-%02X-%02X\n", (int)rchandle.address[0], (int)rchandle.address[1], (int)rchandle.address[2], (int)rchandle.address[3], (int)rchandle.address[4]);
            LOG_D("Load channel:%d\n\n", rchandle.rfChannel);

            rc_into_trans_mode();
        }
    }
}

int rc_task_init(void)
{

    uint8_t rf_error_ret = 0;
    uint8_t rx[5] = {1, 2, 3, 4, 5};

    if (pair_data()->flag == RC_PAIRED_FLAG)
    {
        memcpy(rchandle.address, pair_data()->address, 5);
        rchandle.rfChannel = pair_data()->rf_channel;

        LOG_D("Load address:%02X-%02X-%02X-%02X-%02X\n", (int)rchandle.address[0], (int)rchandle.address[1], (int)rchandle.address[2], (int)rchandle.address[3], (int)rchandle.address[4]);
        LOG_D("Load channel:%d\n", rchandle.rfChannel);
    }

    // si24r1_init();

    hw_si24r1_tx_address(rchandle.address);
    hw_si24r1_rx_address(rchandle.address);
    hw_si24r1_rf_channel(rchandle.rfChannel);

    do
    {
        hw_si24r1_read_reg_buf(SI24R1_REG_RX_ADDR_P0, rx, 5);
        if (memcmp(rx, rchandle.address, 5) == 0)
        {
            LOG_D("rc_task_init: rf chip ok!\n");
            rf_error_ret = 0;
        }
        else
        {
            hw_si24r1_tx_address(rchandle.address);
            hw_si24r1_rx_address(rchandle.address);

            // hw_si24r1_write_reg_buf(SI24R1_REG_RX_ADDR_P0 | SI24R1_CMD_W_REGISTER, rchandle.address, 5);
            LOG_D("check rf device...%d/1 %02X-%02X-%02X-%02X-%02X\n", (int)rf_error_ret, (int)rchandle.address[0], (int)rchandle.address[1], (int)rchandle.address[2], (int)rchandle.address[3], (int)rchandle.address[4]);
            LOG_D("check rf device...%d/2 %02X-%02X-%02X-%02X-%02X\n\n", (int)rf_error_ret, (int)rx[0], (int)rx[1], (int)rx[2], (int)rx[3], (int)rx[4]);
            if (rf_error_ret++ > 100)
            {
                rf_error_ret = 1;
                // RGB_STATE_SET(RGB_STATE_RF_ERROR);
                LOG_D("rc_task_init: rf chip error\n");
                return -1;
            }
        }
        rt_thread_delay(20);
    } while (rf_error_ret);

    rc_into_trans_mode();
    // rc_send_data(&rchandle);
    // os_timer_start(&rc_task_timer, RC_TASK_INTERVAL_MS, 1);
    // rc_send_data(&rchandle);
    // check_all_regs();

    LOG_D("rc_task_init:\n");
    return 0;
}
